# _*_ coding: utf-8 _*_
# @Time : 2020/9/28 18:17
# @Author : xs
# @Version：V 0.1
# @File : CalculateUtils.py
# @desc :
import numpy as np

class CalcuUtils():

    @staticmethod
    def multiList(l, multi):
        new_list = []
        for i in l:
            new_list.append(i * multi)
        return new_list

    # 向量与水平面夹角
    @staticmethod
    def angleWithHorizontal(vector):
        '''
        向量与水平面夹角（XOy）
        法向量：
        XOZ：[0, 1, 0]
        XOY：[0, 0, 1]
        YOZ：[1, 0, 0]
        :param vector:
        :return:
        '''
        # 设平面α的一个法向量为n，直线l的方向向量为a, 直线l与平面α的夹角为θ，则
        # cos(π / 2 - θ) = n·a / [ | n | | a |]
        # sinθ = (a.b) / (| a | | b |)

        # 水平面法向量
        normalVector = np.array([0, 0, 1])
        a = np.dot(vector, normalVector)
        b = np.linalg.norm(vector)
        c = np.linalg.norm(normalVector)
        if b * c == 0:
            return 0
        try:

            # print(a)
            # print(b)
            # print(c)
            devide = np.divide(a, (b * c))
        except Exception as e:
            print(e)
            return 0
        radian = np.arcsin(devide)
        angle = np.degrees(radian)
        return angle


    # 向量与xoz平面的夹角
    @staticmethod
    def angleWithHorizontal_xoz(vector):
        '''
        向量与水平面夹角（XOZ）
        法向量：
        XOZ：[0, 1, 0]
        XOY：[0, 0, 1]
        YOZ：[1, 0, 0]
        :param vector:
        :return:
        '''
        # 设平面α的一个法向量为n，直线l的方向向量为a, 直线l与平面α的夹角为θ，则
        # cos(π / 2 - θ) = n·a / [ | n | | a |]
        # sinθ = (a.b) / (| a | | b |)

        # 水平面法向量
        normalVector = np.array([0, 1, 0])
        a = np.dot(vector, normalVector)
        b = np.linalg.norm(vector)
        c = np.linalg.norm(normalVector)
        if b * c == 0:
            return 0
        try:

            # print(a)
            # print(b)
            # print(c)
            devide = np.divide(a, (b * c))
        except Exception as e:
            print(e)
            return 0
        radian = np.arcsin(devide)
        angle = np.degrees(radian)
        return angle
    # 向量与yoz平面的夹角
    @staticmethod
    def angleWithHorizontal_yoz(vector):
        '''
        向量与水平面夹角（yOZ）
        法向量：
        XOZ：[0, 1, 0]
        XOY：[0, 0, 1]
        YOZ：[1, 0, 0]
        :param vector:
        :return:
        '''
        # 设平面α的一个法向量为n，直线l的方向向量为a, 直线l与平面α的夹角为θ，则
        # cos(π / 2 - θ) = n·a / [ | n | | a |]
        # sinθ = (a.b) / (| a | | b |)

        # 水平面法向量
        normalVector = np.array([1, 0, 0])
        a = np.dot(vector, normalVector)
        b = np.linalg.norm(vector)
        c = np.linalg.norm(normalVector)
        if b * c == 0:
            return 0
        try:

            # print(a)
            # print(b)
            # print(c)
            devide = np.divide(a, (b * c))
        except Exception as e:
            print(e)
            return 0
        radian = np.arcsin(devide)
        angle = np.degrees(radian)
        return angle

    @staticmethod
    def getTemDict(bodyList, retList):
        temDict = {}
        # one frame
        temDict[bodyList[0]] = retList[13]
        temDict[bodyList[1]] = retList[5]
        temDict[bodyList[2]] = retList[9]
        temDict[bodyList[3]] = retList[11]
        temDict[bodyList[4]] = retList[3]
        temDict[bodyList[5]] = retList[7]
        temDict[bodyList[6]] = retList[16]
        temDict[bodyList[7]] = retList[14]
        temDict[bodyList[8]] = retList[6]
        temDict[bodyList[9]] = retList[10]
        temDict[bodyList[10]] = retList[12]
        temDict[bodyList[11]] = retList[4]
        temDict[bodyList[12]] = retList[8]
        temDict[bodyList[13]] = retList[17]
        temDict[bodyList[14]] = retList[15]
        temDict[bodyList[15]] = retList[18]
        temDict[bodyList[16]] = retList[22]
        temDict[bodyList[17]] = retList[19]
        temDict[bodyList[18]] = retList[23]
        temDict[bodyList[19]] = retList[20]
        temDict[bodyList[20]] = retList[21]
        temDict[bodyList[21]] = retList[0]
        temDict[bodyList[22]] = retList[1]
        temDict[bodyList[23]] = retList[2]
        if len(retList)==30:
            temDict[bodyList[24]] = retList[24]
            temDict[bodyList[25]] = retList[25]
            temDict[bodyList[26]] = retList[26]
            temDict[bodyList[27]] = retList[27]
            temDict[bodyList[28]] = retList[28]
            temDict[bodyList[29]] = retList[29]
        # temDict[bodyList[30]] = retList[30]
        # temDict[bodyList[31]] = retList[31]


        return temDict

    @staticmethod
    def getTemDict2(bodyList, retList):
        temDict = {}
        # one frame
        temDict[bodyList[0]] = retList[13].tolist()
        temDict[bodyList[1]] = retList[5].tolist()
        temDict[bodyList[2]] = retList[9].tolist()
        temDict[bodyList[3]] = retList[11].tolist()
        temDict[bodyList[4]] = retList[3].tolist()
        temDict[bodyList[5]] = retList[7].tolist()
        temDict[bodyList[6]] = retList[16].tolist()
        temDict[bodyList[7]] = retList[14].tolist()
        temDict[bodyList[8]] = retList[6].tolist()
        temDict[bodyList[9]] = retList[10].tolist()
        temDict[bodyList[10]] = retList[12].tolist()
        temDict[bodyList[11]] = retList[4].tolist()
        temDict[bodyList[12]] = retList[8].tolist()
        temDict[bodyList[13]] = retList[17].tolist()
        temDict[bodyList[14]] = retList[15].tolist()
        temDict[bodyList[15]] = retList[18].tolist()
        temDict[bodyList[16]] = retList[22].tolist()
        temDict[bodyList[17]] = retList[19].tolist()
        temDict[bodyList[18]] = retList[23].tolist()
        temDict[bodyList[19]] = retList[20].tolist()
        temDict[bodyList[20]] = retList[21].tolist()
        temDict[bodyList[21]] = retList[0].tolist()
        temDict[bodyList[22]] = retList[1].tolist()
        temDict[bodyList[23]] = retList[2].tolist()
        if len(retList)==30:
            temDict[bodyList[24]] = retList[24].tolist()
            temDict[bodyList[25]] = retList[25].tolist()
            temDict[bodyList[26]] = retList[26].tolist()
            temDict[bodyList[27]] = retList[27].tolist()
            temDict[bodyList[28]] = retList[28].tolist()
            temDict[bodyList[29]] = retList[29].tolist()
        # temDict[bodyList[30]] = retList[30].tolist()
        # temDict[bodyList[31]] = retList[31].tolist()
        return temDict

    # 向量在水平面的投影
    @staticmethod
    def vectorProjectionHorizontal(vector):
        # XOZ
        # a - (a · n)n
        # 水平面法向量
        # 法向量：
        # XOZ：[0, 1, 0]
        # XOY：[0, 0, 1]
        # YOZ：[1, 0, 0]
        normalVector = np.array([0, 0, 1])
        vector = vector - np.dot(vector, normalVector) * normalVector
        return vector

    # 向量在yoz面上的投影
    @staticmethod
    def vectorProjectionYOZ(vector):
        normaVector = np.array([1, 0, 0])
        vector = vector - np.dot(vector, normaVector) * normaVector
        return vector

    # 向量在xoz面上的投影
    @staticmethod
    def vectorProjectionXOZ(vector):
        normaVector = np.array([0, 1, 0])
        vector = vector - np.dot(vector, normaVector) * normaVector
        return vector

    @staticmethod
    def vectorProjectionXOY(vector):
        normaVector = np.array([0, 0, 1])
        vector = vector - np.dot(vector, normaVector) * normaVector
        return vector

    # 两个向量的夹角
    @staticmethod
    def vectorAngle(vector1, vector2):
        # cosθ = a dot b / (|a| * |b|)
        try:
            length = (np.linalg.norm(vector1) * np.linalg.norm(vector2))
            if length != 0:
                cos_value = (float(np.divide(np.dot(vector1, vector2), length)))

                eps = 1e-6
                if 1.0 < cos_value < 1.0 + eps:
                    cos_value = 1.0
                elif -1.0 - eps < cos_value < -1.0:
                    cos_value = -1.0
                radian = np.arccos(cos_value)
                angle = np.degrees(radian)


                return angle
            else:
                # print("向量计算时，分母不能为0！")
                return 1
        except Exception as e:
            print(e)
            return 0

    # 体态新增
    # 向量与任意平面夹角
    @staticmethod
    def angleWithRandomPlane(vector, normalVector):
        '''
        向量与任意平面夹角

        param
            vector:空间向量
            normalVector:平面法向量
        :return:
        '''
        # 设平面α的一个法向量为n，直线l的方向向量为a, 直线l与平面α的夹角为θ，则
        # cos(π / 2 - θ) = n·a / [ | n | | a |]
        # sinθ = (a.b) / (| a | | b |)

        # 水平面法向量
        # normalVector = np.array([0, 1, 0])
        a = np.dot(vector, normalVector)
        b = np.linalg.norm(vector)
        c = np.linalg.norm(normalVector)
        if b * c == 0:
            return 0
        try:
            devide = np.divide(a, (b * c))
        except Exception as e:
            print(e)
            return 0
        radian = np.arcsin(devide)

        angle = np.degrees(radian)
        return angle

    @staticmethod
    def cross_product_direction(vector1, vector2):
        '''
        计算两个二维向量的叉积并判断旋转方向

        param
            vector1: 第一个二维向量 (x1, y1)
            vector2: 第二个二维向量 (x2, y2) - 参考向量
        :return: 顺时针返回1，逆时针返回-1
        '''
        # 计算叉积 z = x2*y1 - y2*x1
        cross_product = vector2[0] * vector1[1] - vector2[1] * vector1[0]

        # 判断方向
        if cross_product > 0:
            return -1  # 逆时针
        elif cross_product < 0:
            return 1  # 顺时针
        else:
            return 0  # 共线

    @staticmethod
    def angle_and_direction_with_plane(vector, normal_vector):
        '''
        计算向量与平面的夹角及旋转方向

        param
            vector: 需要计算的空间向量 (x, y, z)
            normal_vector: 平面的法向量 (nx, ny, nz)
        :return: 夹角（度数）和方向（顺时针返回1，逆时针返回-1，共线返回0）
        '''
        # 计算点积
        dot_product = np.dot(vector, normal_vector)
        vector_norm = np.linalg.norm(vector)
        normal_vector_norm = np.linalg.norm(normal_vector)

        # 计算夹角（弧度）
        cos_theta = dot_product / (vector_norm * normal_vector_norm)
        # 夹角（度数）
        angle = np.degrees(np.arccos(np.clip(cos_theta, -1.0, 1.0)))

        # 计算叉积
        cross_product = np.cross(normal_vector, vector)

        # 判断旋转方向，使用z分量来判断
        direction = np.sign(cross_product[-1])  # z轴的分量

        # 返回夹角和方向
        return angle, direction

    @staticmethod
    def direction_on_plane(vector, normal_vector, reference_vector=np.array([1, 0, 0])):
        def project_vector_onto_plane(vector, normal_vector):
            '''
            将向量投影到平面上

            param
                vector: 原始向量
                normal_vector: 平面的法向量
            :return: 向量在平面上的投影
            '''
            normal_vector = normal_vector / np.linalg.norm(normal_vector)
            projection = vector - np.dot(vector, normal_vector) * normal_vector
            return projection
        '''
        判断一个向量相对于平面上的顺时针或逆时针方向位置

        param
            vector: 要判断的向量
            normal_vector: 平面的法向量
            reference_vector: 参考向量，通常选择平面上的某个固定方向（默认为X轴方向）
        :return: 顺时针返回1，逆时针返回-1，共线返回0
        '''
        # 将向量投影到平面上
        projected_vector = project_vector_onto_plane(vector, normal_vector)

        # 计算与参考向量的夹角
        cross_product = np.cross(reference_vector, projected_vector)

        # 通过叉积的符号判断方向
        direction = np.sign(np.dot(cross_product, normal_vector))  # 使用法向量来判断方向

        return direction
if __name__ =="__main__":

    vector1 = np.array([1, 0])
    vector2 = np.array([-1, 0])

    angle = CalcuUtils.vectorAngle(vector1, vector2)
    print(f"The angle between the vectors is: {angle} degrees")
